# Untitled - By: 86166 - 周二 8月 1 2023

import sensor
import lcd
import image
from modules import ybserial
import random
from modules import ybrgb

# 颜色的LAB值
# L: 明亮度: 0-100 [黑,白]
# a: 红绿色: 正值,红色 负值,绿色 [-128, 127]
# b: 黄蓝色: 正值,黄色 负值,蓝色 [-128, 127]
# 差E = 开跟(差L^2 + 差L^2 + 差L^2)


# lcd and sensor 初始化
lcd.init()
sensor.reset()


# sensor初始配置
sensor.set_pixformat(sensor.RGB565)
# sensor.set_framesize(sensor.QVGA)
# 只有在QQVGA的设置下,才可以使用矩形识别 160*120
sensor.set_framesize(sensor.QQVGA)
sensor.run(1)
sensor.skip_frames()
# 颜色捕捉时,设置为False
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)


# 颜色识别阈值
# 黑色底板识别相对稳定LAB区间 [10, 30, 10, 100, -20, 60]
# [49, 78, 30, 65, -11, 18] # 测试白色底板,可以,但黑色底板不行
# black_threshold = [0, 30, -128, 0, -128, 0]
# red_on_black = [10, 30, 10, 100, -20, 60]
# red_on_white = [49, 78, 30, 65, -11, 18]


# 创建串口通信对象
serial = ybserial()
# 创建image对象
img = sensor.snapshot()
# 设置RGB灯光
RGB = ybrgb()
# 创建检测区间
# 该框原点坐标
findLen = [90, 90]
# subXY = [(160 // 2) - (findLen[0] // 2), (120 // 2) - (findLen[1] // 2)] # 以摄像头中心为方框中点
# subXY = [(160 // 2) - (100 // 2), (120 // 2) - (80 // 2)]
subXY = [(img.width() // 2) - (findLen[0] // 2), (img.height() // 2) - (findLen[1] // 2)]
scope = [subXY[0], subXY[1], findLen[0], findLen[1]]
# 存放识别到的矩形数据
rectangleData = []


# 找到最大的矩形
def FindMaxRectangle(rects):
    maxSize = 0
    ret = -1
    for recognize in rects:
        if maxSize < recognize.magnitude():
            maxSize = recognize.magnitude()
            ret = recognize
    return ret


# 绘制识别到的矩形
def DrawRecognizeRectangle(recognize):
    global img
    img.draw_rectangle(recognize.rect())
    for XY in recognize.corners():
        img.draw_circle(XY[0], XY[1], 5, color=(0, 255, 0))


# 将检测到的数据进行留存
def LoadRecognizeData(recognize):
    global rectangleData
    if len(rectangleData) == 50:
        rectangleData.pop(0)
    sure = True
    for XY in recognize.corners():
        if XY[0] < 0 or XY[1] < 0:
            sure = False
    if sure:
        rectangleData.append(recognize)


# 计算方差
def variance(nums):
    if len(nums) == 0:
        return 0
    avg = sum(nums) / len(nums)
    ret = 0
    for i in range(0, len(nums)):
        ret += (nums[i] - avg) ** 2
    return ret / len(nums)

# 对数据进行去噪处理
def DataFilter(nums):
    ret = nums
    ret.sort()
    while variance(ret) > 8:
        ret.pop(0)
        ret.pop(len(ret) - 1)
        if len(ret) == 0:
            break
    return ret

# 对采集到的矩形进行滤波,去除噪点影响
def RectangleFilter():
    global rectangleData

    ret = [[0, 0], [0, 0], [0, 0], [0, 0]]
    if len(rectangleData) > 20:
        for i in range(0, 4):
            retx = []
            rety = []
            for elem in rectangleData:
                XY = elem.corners()
                retx.append(XY[i][0])
                rety.append(XY[i][1])
            retx = DataFilter(retx)
            rety = DataFilter(rety)
            if len(retx) == 0 or len(rety) == 0:
                return [[0, 0], [0, 0], [0, 0], [0, 0]]
            ret[i] = [sum(retx) // len(retx), sum(rety) // len(rety)]
    return ret


# 识别矩形
def FindRectangleInit():
    for i in range(50):
        img = sensor.snapshot()
        recognize = FindMaxRectangle(img.find_rects(threshold=7500, roi=scope))
        if recognize == -1:
            lcd.display(img)
            continue
        LoadRecognizeData(recognize)
        DrawRecognizeRectangle(recognize)
        lcd.display(img)

def XYmiddle(XY1, XY2):
    return [(XY1[0] + XY2[0]) // 2, (XY1[1] + XY2[1]) // 2]

# 运行开始处
FindRectangleInit()
rectangleXY = RectangleFilter()
print(rectangleXY)
#tmp = [rectangleXY[0], XYmiddle(rectangleXY[0], rectangleXY[1]), rectangleXY[1], XYmiddle(rectangleXY[1], rectangleXY[2]), rectangleXY[2], XYmiddle(rectangleXY[2], rectangleXY[3]), rectangleXY[3], XYmiddle(rectangleXY[3], rectangleXY[0])]
#rectangleXY = tmp

sensor.reset()
# sensor配置初始化
# sensor.set_pixformat(sensor.RGB565)
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)
sensor.run(1)
sensor.skip_frames()
sensor.set_auto_exposure(False, 19000)
# 设置摄像头对比度,范围为[-2,+2]
# sensor.set_contrast(2)   # 可以大一点
# 设置摄像头饱和度,范围为[-2,+2]
# sensor.set_saturation(2)   # 可以多给一点
# 设置摄像头亮度，范围为[-2,+2]
# sensor.set_brightness(-3)   # 可以给小一点
# 颜色捕捉时,设置为False
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)

red_on_white = (60, 255, -20, 20, -20, 20)

# 绘制识别到的红点
def DrawRecognizeLaser(laserXY):
    img.draw_rectangle(scope, thicknesss=2)
    if laserXY == [0, 0]:
        return
    laserLen = [10, 10]
    laserRectangle = [laserXY[0] - (laserLen[0] // 2), laserXY[1] - (laserLen[1] // 2), laserLen[0], laserLen[1]]
    img.draw_rectangle(laserRectangle, color=(0, 255, 0), thickness=2)

XYi = 0

# 判断激光是否到达
def LaserXYReach(dXY):
    global recctangleXY, XYi
    if -8 <= dXY[0] <= 8 and -8 <= dXY[1] <= 8:
        XYi += 1
        if XYi == len(rectangleXY):
            XYi = 0

# 灯光熄灭
def LEDOFF():
    RGB.set(0, 0, 0)

# 控制灯光闪烁
def LEDON():
    color = [random.randint(0, 1), random.randint(0, 1), random.randint(0, 1)]
    RGB.set(color[0], color[1], color[2])

# 发送坐标数据
def UARTSendRectangleXY(laserXY):
    global rectangleXY, XYi, serial
    if laserXY == [0, 0]:
        return
    # 发送起始数据
    sendArray = [0xb3, 0xb3, rectangleXY[XYi][0] - laserXY[0], -(rectangleXY[XYi][1] - laserXY[1]), 0x5b]
    # sendArray = [0xb3, 0xb3, rectangleXY[0][0] - laserXY[0],  -(rectangleXY[0][1] - laserXY[1]), 0x5b]
    serial.send_bytearray(sendArray)
    LaserXYReach(sendArray[2:4])
    #print(XYi)
    #print(laserXY[0], laserXY[1])
    #print(rectangleXY[XYi][0], rectangleXY[XYi][1])
    print(sendArray[2:4])
    #print('')

    # 发送坐标
    # for XY in rectangleXY:
    #     # 从左上角开始顺时针返回坐标
    #     sendArray = [0xb3, 0xb3, XY[0] - (img.width() // 2), XY[1] - (img.height() // 2), 0x5b]
    #     serial.send_bytearray(sendArray)
    #     print(sendArray)
    # 发送原始坐标数据
    # for XY in rectangleXY:
    #     # 从左上角开始顺时针返回坐标
    #     sendArray = [XY[0], XY[1]]
    #     serial.send_bytearray(sendArray)
    #     print(XY)
    # print('')

while True:

    # 未找到矩形
    if rectangleXY == [[0, 0], [0, 0], [0, 0], [0, 0]]:
        LEDON()
        continue
    else:
        LEDOFF()

    img = sensor.snapshot()
    for i in range(0, 4):
        img.draw_cross(rectangleXY[i][0], rectangleXY[i][1], color=(0, 255, 0), thickness=2)

    recognize = [0, 0]

    # 找到最大的激光色块
    for elem in img.find_blobs([red_on_white], roi=scope, merge=False, pixels_threshold=1, area_threshold=1):
        if recognize == [0, 0] or recognize.pixels() < elem.pixels():
                recognize = elem

    if recognize == [0, 0]:
        laserXY = [0, 0]
    else:
        laserXY = [recognize.cx(), recognize.cy()]

    DrawRecognizeLaser(laserXY)

    UARTSendRectangleXY(laserXY)

    lcd.display(img)
